// #include "main.h" // Device header
// #include "DriversStruct.h"
// #include "PID.h"

// /*PID初始化*/

// void PID_Init(void)
// {
// 	/*  电机1  */
// 	Platform.motor1.g_Angle_pid.Setpoint = 0;	   /* 设定目标值 */
// 	Platform.motor1.g_Angle_pid.ActualValue = 0.0; /* 期望输出值 */
// 	Platform.motor1.g_Angle_pid.SumError = 0.0;	   /* 积分值 */
// 	Platform.motor1.g_Angle_pid.Error = 0.0;	   /*e[n]*/
// 	Platform.motor1.g_Angle_pid.LastError = 0.0;   /*e[n-1]*/
// 	Platform.motor1.g_Angle_pid.PrevError = 0.0;   /*e[n-2]*/
// 	Platform.motor1.g_Angle_pid.Pidoutput = 0.0;
// 	Platform.motor1.g_Angle_pid.Proportion = Angle_KP1; /* 比例常数  */
// 	Platform.motor1.g_Angle_pid.integral = Angle_KI1;	/* 积分常数 */
// 	Platform.motor1.g_Angle_pid.Derivation = Angle_KD1; /* 微分常数  */
// 	Platform.motor1.g_Angle_pid.ff.tau = Angle_timeConstant1;
// 	Platform.motor1.g_Angle_pid.ff.Ts = 0.0;
// 	Platform.motor1.g_Angle_pid.ff.lasttime = 0.0;
// 	Platform.motor1.g_Angle_pid.ff.refPrev = 0.0;

// 	Platform.motor1.g_Speed_pid.Setpoint = 0;	   /* 设定目标值 */
// 	Platform.motor1.g_Speed_pid.ActualValue = 0.0; /* 期望输出值 */
// 	Platform.motor1.g_Speed_pid.SumError = 0.0;	   /* 积分值 */
// 	Platform.motor1.g_Speed_pid.Error = 0.0;	   /*e[n]*/
// 	Platform.motor1.g_Speed_pid.LastError = 0.0;   /*e[n-1]*/
// 	Platform.motor1.g_Speed_pid.PrevError = 0.0;   /*e[n-2]*/
// 	Platform.motor1.g_Speed_pid.Pidoutput = 0.0;
// 	Platform.motor1.g_Speed_pid.Proportion = Speed_KP1; /* 比例常数  */
// 	Platform.motor1.g_Speed_pid.integral = Speed_KI1;	/* 积分常数 */
// 	Platform.motor1.g_Speed_pid.Derivation = Speed_KD1; /* 微分常数  */
// 	Platform.motor1.g_Speed_pid.ff.tau = Speed_timeConstant1;
// 	Platform.motor1.g_Speed_pid.ff.Ts = 0.0;
// 	Platform.motor1.g_Speed_pid.ff.lasttime = 0.0;
// 	Platform.motor1.g_Speed_pid.ff.refPrev = 0.0;

// 	/*  电机2  */
// 	Platform.motor2.g_Angle_pid.Setpoint = 0;	   /* 设定目标值 */
// 	Platform.motor2.g_Angle_pid.ActualValue = 0.0; /* 期望输出值 */
// 	Platform.motor2.g_Angle_pid.SumError = 0.0;	   /* 积分值 */
// 	Platform.motor2.g_Angle_pid.Error = 0.0;	   /*e[n]*/
// 	Platform.motor2.g_Angle_pid.LastError = 0.0;   /*e[n-1]*/
// 	Platform.motor2.g_Angle_pid.PrevError = 0.0;   /*e[n-2]*/
// 	Platform.motor2.g_Angle_pid.Pidoutput = 0.0;
// 	Platform.motor2.g_Angle_pid.Proportion = Angle_KP2; /* 比例常数  */
// 	Platform.motor2.g_Angle_pid.integral = Angle_KI2;	/* 积分常数 */
// 	Platform.motor2.g_Angle_pid.Derivation = Angle_KD2; /* 微分常数  */
// 	Platform.motor2.g_Angle_pid.ff.tau = Angle_timeConstant2;
// 	Platform.motor2.g_Angle_pid.ff.Ts = 0.0;
// 	Platform.motor2.g_Angle_pid.ff.lasttime = 0.0;
// 	Platform.motor2.g_Angle_pid.ff.refPrev = 0.0;

// 	Platform.motor2.g_Speed_pid.Setpoint = 0;	   /* 设定目标值 */
// 	Platform.motor2.g_Speed_pid.ActualValue = 0.0; /* 期望输出值 */
// 	Platform.motor2.g_Speed_pid.SumError = 0.0;	   /* 积分值 */
// 	Platform.motor2.g_Speed_pid.Error = 0.0;	   /*e[n]*/
// 	Platform.motor2.g_Speed_pid.LastError = 0.0;   /*e[n-1]*/
// 	Platform.motor2.g_Speed_pid.PrevError = 0.0;   /*e[n-2]*/
// 	Platform.motor2.g_Speed_pid.Pidoutput = 0.0;
// 	Platform.motor2.g_Speed_pid.Proportion = Speed_KP2; /* 比例常数  */
// 	Platform.motor2.g_Speed_pid.integral = Speed_KI2;	/* 积分常数 */
// 	Platform.motor2.g_Speed_pid.Derivation = Speed_KD2; /* 微分常数  */
// 	Platform.motor2.g_Speed_pid.ff.tau = Speed_timeConstant2;
// 	Platform.motor2.g_Speed_pid.ff.Ts = 0.0;
// 	Platform.motor2.g_Speed_pid.ff.lasttime = 0.0;
// 	Platform.motor2.g_Speed_pid.ff.refPrev = 0.0;
// }

// // 计算前馈补偿输出
// double FeedforwardCompensator_Compute(FeedforwardCompensator *ff, double refSignal)
// {
// //	if (ff->tau + ff->Ts == 0)
// //	{
// 		// 若出现除零风险，返回0并给出提示
// 		return 0.0;
// //	}

// 	// 一阶系统的简单前馈补偿
// //	return (refSignal - ff->refPrev) / (ff->tau + ff->Ts) + ff->refPrev / (ff->tau + ff->Ts) * ff->tau;
// }

// /*PID闭环控制*/

// float Pid_closed_control(PID_TypeDef *PID, float Feedback_value) // Feedback_value实际值
// {
// 	// 1. 计算时间间隔（自动处理时间戳）
// 	uint32_t now = HAL_GetTick();
// 	float dt = (float)(now - PID->ff.lasttime) / 1000.0f; // 转换为秒

// 	// 处理首次调用和极小时间间隔
// 	if (dt <= 0.001f)
// 	{								// 最小时间间隔保护
// 		dt = 0.001f;				// 1ms最小时间间隔
// 		PID->ff.lasttime = now - 1; // 维持有效时间差
// 	}
// 	else
// 	{
// 		PID->ff.lasttime = now; // 更新时间戳
// 	}

// 	PID->ff.Ts = dt;

// 	PID->Error = (float)(PID->Setpoint - Feedback_value); // 误差等于目标值-实际值

// #if SELECT // 增量式
// 	PID->pidoutput += (PID->Proportion * (PID->Error - PID->LastError)) + (PID->integral * PID->Error) + (PID->Derivation * (PID->Error - 2 * PID->LastError + PID->PrevError));

// 	PID->PrevError = PID->LastError;
// 	PID->LastError = PID->Error;
// #else // 位置式

// 	PID->SumError += PID->Error; // 误差累计
// 	if (PID->SumError >= 2000)
// 	{
// 		PID->SumError = 2000;
// 	}
// 	if (PID->SumError <= -2000)
// 	{
// 		PID->SumError = -2000;
// 	}

// 	PID->Pidoutput = (PID->Proportion * PID->Error) + (PID->integral * PID->SumError) + (PID->Derivation * (PID->Error - PID->LastError));
// 	PID->LastError = PID->Error;

// #endif
// 	double u_ff = FeedforwardCompensator_Compute(&PID->ff, PID->Setpoint);

// 	// 更新参考输入
// 	PID->ff.refPrev = PID->Setpoint;

// 	PID->ActualValue = PID->Pidoutput + u_ff;

// 	return ((float)(PID->ActualValue));
// }
